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Abstract 

This report addresses the problem of path planning and control of robotic manipulators 
which have joint-position limits and joint-rate limits. The manipulators move autonomously 
and carry out variable tasks in a dynamic, unstructured and cluttered environment. The 
issue considered is whether the robotic manipulator can achieve all its tasks, and if it can- 
not, the objective is to identify the closest achievable goal. This problem is formalized and 
systematically solved for generic manipulators by using inverse kinematics and forward kine- 
matics. Inverse kinematics are employed to define the subspace, workspace and constrained 
workspace, which are then used to identify when a task is not achievable. The closest achiev- 
able goal is obtained by determining weights for an optimal control redistribution scheme. 
These weights are quantified by using forward kinematics. 

Conditions leading to joint rate limits are identified, in particular it is established that all 
generic manipulators have singularities at the boundary of their workspace, while some have 
loci of singularities inside their workspace. Once the manipulator singularity is identified 
the command redistribution scheme is used to compute the closest achievable Cartesian 
velocities. Two examples are used to illustrate the use of the algorithm: A three link 
planar manipulator and the Unimation Puma 560. Implementation of the derived algorithm 
is effected by using a supervisory expert system to check whether the desired goal lies in 
the constrained workspace and if not, to evoke the redistribution scheme which determines 
the constraint relaxation between end effector position and orientation, and then computes 
optimal gains. 
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1 Introduction 


In this section the problem of optimal tracking of joint commands while experiencing satu- 
rated actuators is explained and discussed. Previous research work in this area is reviewed, 
in particular the work addressing the accommodation of actuator limits in multivariable 
systems and the Windup Feedback Scheme [1] is introduced. 

1.1 Accommodation of Actuator Saturation 

Actuator saturation can cause significant deterioration in control system performance be- 
cause unmet demand may result in sluggish transients and oscillations in response to set-point 
changes. Generally, some type of linear control scheme is designed for a system such that 
the control responsibility is divided up among the actuators. When an actuator saturates, 
the linear controller may act in a nonlinear manner and abnormal performance can result. 
This performance may be unacceptable from a safety, cost, or quality standpoint. 

An actuator is a physical system so its output, which under normal conditions is a 
function of its input, is restricted to lie within some boundaries. At the edge of its range of 
motion, additional input to drive it past the endpoint will have no effect. Thus, even though 
the control system is demanding more actuation, the effector cannot provide it. Additionally, 
because of power and wear considerations, the actuator must not be forced against its limit 
for extended periods. In order to assure this, the command to it must be limited so that it 
never tries to drive the actuator outside of its unrestricted range. As long as an actuator 
command stays within the normal bounds, there is no difference between the desired and 
achievable actuator positions. However, as an actuator command moves beyond the normal 
range it gets clipped, indicating that a portion of the control demand can not be met. 

To help compensate for this problem, a technique has been developed which takes ad- 
vantage of redundancy in multivariable systems to redistribute the unmet control demand 
over the remaining useful effectors [1]. This method, the Windup Feedback Scheme, is not 
a redesign procedure, rather it modifies commands to the effectors with remaining author- 
ity to compensate for those which are limited, thereby exploiting the built-in redundancy. 
The original commands are modified by the increments due to unmet demand, but when a 
saturated effector comes off its limit, the incremental commands disappear and the original 
unmodified controller remains intact. This scheme provides a smooth transition between 
saturated and unsaturated modes as it divides up the unmet requirement over any available 
actuators. This way, if there is sufficiently redundant control authority, performance can be 
maintained. 

1.2 Accommodation of Manipulator Joint Limits 

The work discussed above has been extended to estab ish a technique for compensating rate 
and position limits in the joints of a six degree-of-fieedom robotic manipulator [2], The 
unmet demand as a result of actuator saturation is redistributed among the remaining un- 
saturated joints. The scheme is used to compensate for inadequate path planning, problems 
such as joint limiting, joint freezing, or even obstacle avoidance, where a desired position and 
orientation are not attainable due to an unrealizable joint command. Once a joint encoun- 
ters a limit, supplemental commands are sent to other joints to best track, according to a 
selected criterion, the desired trajectory. A standard six degree-of-freedom manipulator has 
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Required joint angles from Inverse Kinematics 
(Unlimited joint commands) 



Windup feedback gains 


Figure 1: Windup Feedback Scheme 

six independently controlled joints. The position and orientation of the end effector, each 
of which is described in three dimensions, are fully determined by the angles of the joints. 
As long as the appropriate joint angles are achievable, the desired position and orientation 
can be obtained. However, when the specified joint trajectories cannot be followed due to a 
command beyond the range of the actuator, positions and orientations downstream from the 
limited joint will all be affected, causing in some cases extreme deviations from the expected 
values. The Windup Feedback Scheme, shown in Figure 1, was designed to compensate 
for actuator saturation in a multivariable system by supplementing the commands to the 
remaining actuators to produce the desired effect on the output. In this case the output 
consists of the gripper position and orientation. For each joint which saturates, a degree of 
freedom is lost, but the remaining joints can be used to track the desired path within the 
physical limits of the manipulator. 
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2 Problems of Joint-Position Limits and Joint-Rate 
Limits 

The objective of the work presented in this report is to systematically address the problem 
of path planning of robotic manipulators with joint-position limits and joint-rate limits. 
This entails a manipulator that moves autonomously in a dynamic environment and carries 
out variable tasks, including picking up randomly scattered items. The environment is 
unstructured and cluttered. The question considered is whether the robotic manipulator 
can achieve all of its desired goals, all the time. If it does not, what is the closest achievable 
goal and how can this be attained? This problem arises when the desired position and 
orientation of the manipulator end-effector requires joint-positions which are not attainable. 

Beyond the path planning problem, in many applications it is important to reach a 
goal position and orientation with a particular (static) velocity. This raises the question 
of converting from Cartesian velocities (goal frame velocities) to the required joint-rates 
(velocities). The inverse of the Jacobian matrix is used to carry out this transformation. 
If the Jacobian matrix is not invertible it means the required Cartesian velocities are not 
achievable, implying joint-rate limits. 


3 Manipulator Spaces and Kinematics 

In this section the notions of actuator space, joint space, Cartesian space and manipulator 
kinematics are introduced. The identification of joint limits is then accomplished by using 
inverse kinematics to define the subspace, workspace and constrained workspace for robotic 
manipulators. 

3.1 Actuator Space, Joint Space and Cartesian Space 

The position of all the links of a manipulator of n degiees of freedom can be specified with a 
set of n joint variables. This set of variables is often referred to as the n x 1 joint vector. The 
space of all such joint vectors is referred to as joint space. The Cartesian space is the space 
that contains all the end-effector positions and orientations where the position is measured 
along orthogonal axes. This space is also referred to as task-oriented space or operational 
space. Each kinematic joint is moved directly or indirectly by some sort of actuator. In 
some cases two actuators work together as a differential pair to move joints. The notion of 
actuator values leads to the definition of the actuator space as that space which contains all 
the actuator vectors, where an actuator vector is a se-; of actuator values. 

3.2 Manipulator Kinematics 

Kinematics is defined as the “geometry of motion”, the branch of dynamics which treats 
motion without regard to forces which cause it. It studies position, velocity, acceleration 
and all higher order derivatives of position variables. Forward kinematics (F.K.) is the static 
geometrical problem of computing the position and orientation of the end-effector of the 
manipulator. Specifically, given a set of joint angles, the forward kinematic problem is to 
compute the position and orientation of the the tool frame relative to the base frame [3]. 
Put differently, forward kinematics involve changing the representation of the manipulator 
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Figure 2: Manipulator Spaces and Kinematics 


position from the joint space description into a Cartesian space description as illustrated in 
Figure 2. The inverse kinematics (I.K.) problem is posed as follows: Given the position and 
orientation of the end-effector of the manipulator, calculate all the possible sets of joint angles 
which could be used to attain this given position and orientation. This is a fundamental 
problem in the practical use of manipulators. This can also be understood as changing the 
representation of the manipulator position from the Cartesian space description into a joint 
space description as illustrated in Figure 2 . 

3.3 Identification of Joint Limits by Inverse Kinematics 

Unlike forward kinematics, the inverse kinematic problem is not simple. The inverse kine- 
matics equations are nonlinear, their solution is not always easy or even possible in a closed 
form. Also the questions of existence of a solution, and of multiple solutions, arise. The 
issue of the existence of a solution or lack of it answers the question of whether goal (or task) 
is attainable. 

Inverse kinematics are used to define subspace, workspace and constrained workspace 
for the robotic manipulator. The workspace is defined as that volume of space which 
the end-effector of the manipulator can reach with fixed joint lengths and no joint limits 
(0 < 0j < 360 deg). Subspace is then defined as the workspace of a generic robot with in- 
finitely variable joint lengths and no joint limits (0 < 6j < 360 deg). Constrained workspace 
is defined as the workspace where the robotic manipulator has fixed joint lengths and joint 
limits ^ ^ ®max) • These spaces are used to solve the problem of identifying when 

a task of the manipulator is not achievable. The relationship between these spaces is sum- 
marized as follows, 

SPACE □ SUBSPACE □ WORKSPACE □ CONSTRAINED WORKSPACE 

For example, a 6 DOFs manipulator’s subspace is the entire 3-D space while its workspace 
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is a portion of 3-D space and its constrained workspace is an even smaller portion of 3-D 
space. Similarly, a three link planar manipulator’s subspace is the entire generic plane and 
its workspace and constrained workspace are decreasing portions of the plane. 

The issue of the closest achievable goal is then addressed by using systematically derived 
weights in the Windup Feedback Scheme. The weights determine the feedback gains shown 
in Figure 1. A derivation of their relationship with the optimal gains appears in previous 
work [2], These weights are obtained and quantified by using forward kinematics, which are 
used to show which joint angles affect position and orientation. Thus defining, for example, 
which joint angle requirements may be compromised or ignored in order to obtain the goal 
position while being flexible about the orientation. In a path planning problem the position 
is more critical than the orientation. Orientation becomes important when the problem of 
obstacle avoidance is also considered. 


4 Resolution of Joint Rate Limits 

When the Jacobian matrix is not invertible it means the required Cartesian velocities are not 
achievable, implying joint rate limits. The conditions under which these limits (singularities 
of manipulator mechanisms) occur, are systematically identified. All generic manipulators 
have singularities at the boundary of their workspace, while some have loci of singularities 
inside their workspace [3]. As indicated before, the manipulator workspace is derived by 
using inverse kinematics. A workspace boundary singularity occurs when the manipulator 
is fully stretched out or folded back. This occurs when the end effector is near or at the 
boundary of its workspace. A workspace interior singularity occurs away from the workspace 
boundary, and is caused by two or more joint axes lining up. A manipulator in a singular 
configuration has lost one or more degrees of freedom. Once the manipulator singularity is 
identified the Windup Feedback Scheme is used to compute the closest achievable Cartesian 
velocities (goal frame velocities). The Jacobian is a function of joint angles which transforms 
joint velocities into Cartesian velocities. The inverse of the Jacobian function facilitates the 
reverse operation, i.e., transforms the Cartesian velocities into joint velocities (joint-rates). 

v(fc) = j{©(fc)>e<(fc) (i) 

=t-e(k) = J-'l@(k)}\'(k) (2) 

Consequently, if the Jacobian is not invertible it means that there are joint-rate limits (sin- 
gularities of the mechanism). 

5 Expert Systems Concepts 

An expert system is a computer program using expei t knowledge to attain high levels of 
performance in a narrow problem area. The process of building expert systems is called 
Knowledge Engineering. This is an integral part of the field of Artificial Intelligence ; a part 
of Computer Science involving the development of intelligent computer programs . Backward 
chaining is an inference method where the system starts with what it wants to prove, e.g. 
Z, and tries to establish the facts it needs to prove Z. Forward chaining is an inference 
method where rules are matched against facts to establish new facts. Expert systems exhibit 
intelligent behavior by skillful application of heuristics. 
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Figure 3: The Role of The Expert System 
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Expert Systems apply expert knowledge to real world problems. Such applications in- 
clude diagnosis, interpretation, prediction, design, planning, control and supervision. Expert 
systems are advantageous over human experts because their knowledge is permanent, easy 
to transfer, easy to document, consistent, predictable and affordable. However they also 
have disadvantages with respect to human experts; lack of creativity, inability to adapt and 
the absence of both common sense knowledge and sensory experience. 

5.1 A Supervisory Expert System 

Implementation of the derived algorithms was done by way of a supervisory expert system. 
In the application of interest, the expert system is used to check whether the goal lies in 
the constrained workspace, if not the expert system evokes the Windup Scheme, decides on 
the constraint relaxation between end effector position and orientation (depending on task) 
and then systematically computes and assigns Windup Feedback gains. The expert system 
design and implementation was carried out in the language CLIPS 5.1 and LISP. 

5.2 An Expert System for the Windup Feedback Scheme 

The problem of path planning when there are joint-position limits is resolved by the su- 
pervisory expert system. The flowchart in Figure 3 illustrates and summarizes the role of 
the expert system. The criteria used in choosing the ‘best solution’ include the following 
factors: closest solution, minimum energy, fastest optio n and the movement of smaller joints 
first. If the goal does not lie in the constrained workspace this means that the goal is not 
totally attainable, i.e, ®i(k) 7^ ® un (k). In this case the Windup Feedback Scheme is then 
evoked. The expert system determines constraint relaxation between end effector position 
and orientation, depending on the specific task. In this way, it systematically computes the 
Windup Feedback gains. 

6 Examples 

Two detailed examples are used to illustrate and demonstrate the supervisory expert system 
algorithm; A three link planar manipulator and the Lnimation PUMA 560. These robotic 
manipulators were chosen because they have simple kinematics which adequately manifest 
the problems of joint-position limits and joint-rate limits. 

6.1 A Three Link Planar Manipulator (3R Mechanism) 

The three link planar manipulator moves in a plane and consists of three links, three revolute 
joints and three parallel axes of rotation as illustrated in Figure 4 . Its subspace is the generic 
plane and its workspace a circular plane defined by the links and joints. L\, L2 and L3 are 
the link lengths, x and y represent the position of the base of the end-effector, and $ is the 
orientation of the end-effector. The joint angles (revo.ute joints) are represented by 6\ . 62 
and 0 3 . 
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Subspace 


The subspace of the three link planar manipulator is 
by the following structure 

cos<j> —sincp 0 

sincj) cos(j) 0 

0 0 1 

0 0 0 


the generic plane which is represented 
x 

y 

0 ’ 

1 


where the variables x, y, and (p take arbitrary values. 


Workspace 

The corresponding workspace for the three link planar manipulator, when there are no joint 
limits, is of the form 


COs(9\ + 02 + $3) 
sin(6\ 4- 62 + $3) 
0 
0 


— sin{0\ + $2 + $3) 0 
cos{9\ + 9-2 + $3) 0 

0 1 

0 0 


L\cos6 1 -f- L/2Cos(6\ + $2) 

Lisindi + L 2 sm(^i + 9 2 ) 
0 

1 
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No joint 
limits 


0 <0i <360° 

=> O<0 2 <360° where Li, L 2 , L 3 = constants 
0 <0 3 <360° 


Constrained Workspace 

When there are joint limits the workspace is reduced, resulting in the constrained workspace 
which is represented as follows 


cos(Q i + 02 + $3) — sin{6\ 4- 62 + $3) 0 L\cos0i + Z, 2 cos(0i + 62) 
sin{6\ + 02 d~ ^3) cos(0j + 0 2 + 03 ) 0 L/\sind\ 4- Li2sin(9\ 4- 0 2 ) 

0 0 1 0 

0 0 0 1 


Joint 

limits 


0 , < 0 , < 0 , 

Arntn — 1 1 max 

02 mtn <02 <02 mal 

0.3— <03 <03n 


where Ly, L 2 , L3 = constants 


Inverse Kinematic Solution 


The generic end-effector frame or general goal is given by the generic homogeneous transfor- 
mation matrix, 


W 


T = 


l ,R 


w 


T = 


A P, 


Borg 


0 

0 

0 : 

1 

- 

n 1 

02 

03 

: x 


r 2 1 

T 22 

03 

: y 

True for any 

r 31 

r 32 

r 33 

: 2 

end- effector 
in 3-D Space 

0 

0 

0 

: 1 



The following are the conditions to be satisfied for a solution to exist: 


(1) = SUBSPACE (must have the structure of the SUBSPACE) 

(2) = WORKSPACE (must be solvable for joint mgles 0i, 0 2 • • • 0 n ) 

(3) V ^T = CONSTRAINED WORKSPACE (must be solvable for acceptable joint angles 

01 , 02 


Condition (3) is the tightest test. It is the necessary and sufficient condition. 
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For the 3-link planar manipulator 


B 

W 


T = 


coscp —simp 0 x 

sincp cos<j) 0 y 

0 0 10 

0 0 0 1 


= SUBSPACE 


Constrained Workspace Test 


To test whether required joint angles lie within the constrained workspace the subspace 

is equated to the constrained workspace. The resulting equations are then solved for the 
joint angles d u 0 2 and 0 3 . 


C0 

—s<p 

0 

x " 


c 123 

~ s 123 

0 

L\C\ + L 2 C\2 

S(f) 

C(p 

0 

y 


s 123 

c 123 

0 

L\S\ + L 2 S\ 2 

0 

0 

1 

0 


0 

0 

1 

0 

0 

0 

0 

1 _ 


0 

0 

0 

1 


The operator Atan2 (y, x) computes the inverse tangent function, tan ~ l (y/x), but uses the 
signs of both x and y to determine the quadrant in which the resulting angle lies. Hence, 
the inverse kinematic solution gives the following results: 

0 2 = Atan2(s 2 , c 2 ) 

0i = Atan2(?/, x) - Atan2(/q, k 2 ) 

03 = 0 - 0i - 0 2 


s 2 = ±^/l - 4 
k\ — L\ + L2C2 


k >2 — L2S2 


The following are the conditions required to be met for these solution equations to hold 
(exist). 

(a) Li > 0, L 2 > 0 

(b ) -1 < < 1 
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=> 0 2 = Atan 2(s 2 , c 2 ) 


(c) 

0 2 

< 

02 < 02 

^ max 

j joint limits 

; test. 

(d) 

if x = 

= y 

— 0 => 0i is 

arbitrary 0 

< 0i < 360° 

else 

9 1 

=Atan2(y, x) - 

Atan2 (/c 2 , 

ki) 

(e) 

0i 

tmin 

< 

8\ < 0i mai 

] 



h = 

4> 

— @2 — 



(f)[ 

0 3 

< 

03 < 9 3max 




Determination of Weights for Windup Scheme by Expert System 

The goal is expressed as 

x L\Ci + T 2 c 12 

y = L 1 S 1 + L 2 S 12 , 

<P 9\ + 62 + $3 

where x,y define the position of the base of the end-effector and 4> defines the orientation of 
the end-effector. Only 0 X and 0 2 are important for the position. The joint angles 0i, 82 and 
0 3 are all important for the orientation. Consequently, weights can be used to penalize errors 
in 0i and 0 2 for a path planning problem. Since 83 does not affect the position, errors in it 
are less important. Forward kinematics are used to establish how position and orientation 
depend on joint angles, such that joint angles that do not affect position are identified. The 
expert system uses these facts in determining the weights of the Windup Feedback Scheme. 

6.2 The Unimation PUMA 560 (6 DOFs Manipulator) 

The Puma 560 is a rotary joint manipulator with six revolute joints and six degrees of 
freedom. Its joint axes 4, 5 and 6 all intersect at t. common point. Furthermore, these 
joint axes 4, 5 and 6 are all mutually orthogonal establishing the Puma wrist mechanism. 
The frame assignments for a general PUMA 560 are shown in Figure 5, where frame 0 and 
frame 1 are set as coincident. The subspace of the PUMA is the generic 3-D space, but its 
workspace is limited by its link lengths and joint limits to a portion of 3-D space. 

Subspace 

The subspace of the PUMA is the generic 3-D space which is represented as follows 

t n Tyi r 13 : X 

r 2 1 r 22 r 23 : y 

Uil r 32 r 33 : z 


0 0 0:1 
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A 



Figure 5: Frame assignments for the PUMA 560 manipulator 


This is the general goal frame of the end-effector. The workspace is of the same form but it 
is limited to a portion of 3-D space by the finite link lengths of the PUMA. 

Constrained Workspace 

When there are joint limits the PUMA workspace is reduced to a constrained a workspace 
of the form 


“no i) jm) 3 r w Jtw 5 e T(t> 6 ), 

where 
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0 

1 


T = 


1 

2 


T 


2 

3 


T = 


3 

4 


T 


4 

5 


T 


5 

6 


T = 


c6\ 

— s6i 

0 

0 ' 

s6\ 

cQ i 

0 

0 

0 

0 

1 

0 

0 

0 

0 

1 . 

C0 2 

— S 02 

0 

0 

0 

0 

1 

0 

—s8 2 

-c8 2 

0 

0 

0 

0 

0 

1 

C0 3 

-50 3 

0 

a 2 

503 

C0 3 

0 

0 

0 

0 

1 

d 3 

0 

0 

0 

1 

C0 4 

— S04 

0 

a 3 

0 

0 

1 

d 4 

— S04 

— C 0 4 

0 

0 

0 

0 

0 

1 

C0 5 

-50 5 

0 

0 

0 

0 

-1 

0 

505 

C0 5 

0 

0 

0 

0 

0 

1 

C0 6 

-S 0 6 

1 ) 

0 

0 

0 


0 

-50 6 

-C0 6 

0 

0 

0 

0 

0 

1 


The joints are limited such that 0 min < 0 < 0 mai , where the vector of joint angles is 
given by 


'01 ' 
02 

03 

04 

05 

06 


Constrained Workspace Test 

To test whether required joint angles lie within the constrained workspace the subspace is 
equated to the constrained workspace as follows, 
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r 12 r u 


x 


ni 

7*21 

*31 


^ 22 
r 32 


^23 

^33 


y 

z 


= ®m) 2^(02) jT(« 3 ) ®r(» 4 ) jT( 9 5 ) gT(# 6 ). 


0 0 0 : 1 


The resulting equations are solved for the joint angles 9 U 9 2 , 9 3 , 9 4 , 9, and 9 6 , such that the 
following condition is satisfied, 


©min — ® — ©max- (3) 

As a consequence the following results are obtained, 

=Atan2(r/, x) — Atan2(d 3 , ±^/x 2 + y 2 — df) 

9 2 = 9 23 — # 3 

#23 =Atan2[(— a 3 — a 2 c 3 )z — ( c 4 x + Siy)(d 4 — a 2 s 3 ), (a 2 s 3 — d 4 )z — (a 3 + a 2 c 3 )(cix + Siy)] 
and 

0 3 =Atan2(o 3 , d 4 ) — Atan2(A, ±\Ja% + d 2 — A 2 ) 

iv- i 2 + J/ 2 + z 2 - a 2 - a% - d% - dj 

n ~ 2a 2 

^4 = Atan2(— ri 3 s 4 + r 23 Cj, — ^i 3 CiC 23 — r 23 sic 23 + r 33 s 23 ) 

0, = Atan2(s 5 , c 5 ) 

6> 6 = Atan2(s 6 , c 6 ) 


s 6 — r ll [ c 1 c 23 5 4 SlC 4 ] — J"2l[ s l c 23 s 4 + CiC 4 ] + r 3 i[s 23 S 4 ] 

c 6 = rn[(cic 23 c 4 + SlS 4 )c 5 - CiS 23 S 5 ] + r 21 [(sic 23 c 4 - Cxs 4 )c 5 - s^ss] 

— r 3l[ s 23C 4 C5 + C 23 Ss] 

The plus-or-negative signs appearing in the expressions of 9\ and 0 3 lead to four solutions. 
Additionally there are four more solutions obtained by flipping the wrist of the manipulator. 
For each of the four solutions computed above, the flipped solution is given by 

ff 4 = 9 4 + 180° 

O', = -0 5 

9' 6 = 9 6 + 180°. 

For a given position and orientation of the PUMA end-effector there are eight I.K solu- 
tions, i.e, eight sets of joint angles. Four of these are shown in Figure 6. Some or all of the 


15 





Figure 6: Four I.K. Solutions for the PUMA 560 

solutions may be discarded because of joint limit violations. If all the solutions are discarded 
then the Windup Feedback Scheme is used to obtain the closest achievable solution. If there 
are multiple solutions the closest solution is picked whe 'e the criteria of choice might include: 
least energy use, obstacle avoidance, movement of smaller links and least movement of links. 


Determination of Windup Feedback Weights Using Expert System 

Equating the following two matrices and solving the equations produced allows the determi- 
nation of the Windup Feedback Scheme weights. 


m 

T \ 2 

r 13 

: x 


a n 

ai2 

«13 

Pr ' 

r 2 1 

r 22 

r 23 

y 


a 2 i 

a 2 2 

0-23 

Py 

r-3i 

r 32 

^33 

: z 


a 31 

«32 

O 33 

P z 

0 

0 

0 

: 1 _ 


0 

0 

0 

1 


an — c l[ c 23( c 4 c 5 c 6 — 5 4 s 6) ~ S 23 S 5 C 6] + s l( s 4 c 5 c 6 + c 4 5 6) 
a 12 = Ci[c23( — C4C5S6 — S4C6) + S23S5S6] + Si (C4 Cg — S4C5S6) 
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a 13 = — c l(C23 c 4S5 + 523C5) — S1S4S5 

«21 = 5 i[c 23 (c 4 C 5 C 6 - S 4 S 6 ) - S 2 3 S 5 C 6 ] - Ci(s 4 C 5 C 6 + C 4 S 6 ) 
a 22 = Sj[c 2 3 ( — C 4 C 5 S 6 — S 4 C 6 ) + S 23 S 5 S 6 ] — Ci(c 4 C 6 — S 4 C 5 S 6 ) 

°23 = — 5i(c23C 4 S5 + S 23C5) + CiS 4 Ss 
°31 = — S 23( C 4 C 5 C 6 — S 4 S 6) — C23S5C6 
°32 = — ^23 ( — C4C556 — S 4 C6) + C23S556 
a 33 = S23C 4 S5 — C23C5 

P x = x = Cl [a 2 c 2 + 03^23 — ^ 4 ^ 23 ] — d^s\ 

Py = y = Si[a 2 c 2 + 03C23 — d^s^] + d^Ci 

P z = z = -a 3 s 23 - a 2 s 2 - d 4 c 23 

Position of tool base depends on 9\ , 0 2 and 9 3 only. 

It is important to note that the coordinates [ x y z] T specify the position while the angles 
[7 (3 a] r (roll, pitch and yaw) specify the orientation. This is the general representation of 
a generic manipulator in 3-D space. For the PUMA 560 these angles are computed as follows, 

(3 = Atan2(— r 31 , ^fr\ x + r|j) 

a = Atan2(r 2 i/c/?, r n /cP) 

7 =Atan2 (r 32 /c/?, r 33 /c/?). 

From forward kinematics, the position coordinates are functions of joint angles 9\. 9 2 and 
0 3 only, while the orientation is a function of all joint angles. Thus, the feedback weights are 
computed while taking cognizance of these facts. For example, in a path planning problem 
(where the orientation is not critical) errors in 9 \ , 9 2 and 0 3 are more heavily penalized than 
errors in 0 4 , 0 5 and 0 6 . 


7 Conclusions 

The problems of joint-position limits and joint-rate limits in robotic manipulators have 
been addressed by using inverse kinematics and forward kinematics in conjunction with a 
supervisory expert system. Inverse kinematics were employed to define the subspace, the 
workspace and the constrained workspace, which were then used to identify whether or not 
a manipulator task is achievable. The closest achievable goal is obtained by using weights 
in the conventional Windup Feedback Scheme where these weights are quantified by using 
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forward kinematics. It has been shown that robotic manipulators have singularities at the 
boundaries of their workspace, while some have loci of s ingularities inside the workspace. At 
the manipulator singularity, the Windup Feedback Scieme is used to compute the closest 
achievable Cartesian velocities. A three link planar rot otic manipulator and the Unimation 
Puma 560 were effectively used to illustrate the theory c eveloped. Future work might include 
considering robot manipulator dynamics and forces that cause motion which are neglected 
in kinematics, i.e., go beyond static (joint and Cartesian) positions, static forces and static 
velocities. 
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